clear; close all; clc;

saveArg = 0;

xCorrect = 23;
yCorrect = 86.542;
medSize = 110;
labelSzAx = 16;
Lb = 0.194;
stdThreshold = 0.2;


%% Reading data

mark = round(50/70*medSize,0);

% Buoyant non-feeder
data_nf = importdata('..\Exp_Prev_C1\Data_24-10200-10300\B00001.dat');
data_nf = data_nf.data;
xn = (data_nf(:,1) - xCorrect)/1000;
yn = (data_nf(:,2) - yCorrect)/1000;
un = data_nf(:,3);
vn = data_nf(:,4);
noInd = find(un==0&vn==0);
xn(noInd) = [];
yn(noInd) = [];
un(noInd) = [];
vn(noInd) = [];

xlin = linspace(min(xn),max(xn),medSize);
ylin = linspace(min(yn),max(yn),medSize);
xnM = nan(medSize,medSize);
ynM = nan(medSize,medSize);
unM = nan(medSize,medSize);
vnM = nan(medSize,medSize);
for i = 2:medSize-1
    for j = 2:medSize-1
        indCheck = xn>xlin(i-1) & xn<xlin(i+1) & yn>ylin(j-1) & yn<ylin(j+1);
        xCheck = xn(indCheck);
        yCheck = yn(indCheck);
        vxCheck = un(indCheck);
        vyCheck = vn(indCheck);
        xnM(j,i) = median(xCheck(isnan(xCheck)==0));
        ynM(j,i) = median(yCheck(isnan(xCheck)==0));
        unM(j,i) = median(vxCheck(isnan(xCheck)==0));
        vnM(j,i) = median(vyCheck(isnan(xCheck)==0));
    end
end
xn = xnM;
yn = ynM;
un = unM;
vn = vnM;
clear *M *Check;

vector_n = sqrt(un.^2+vn.^2);
theta_n = acos(un./vector_n);

theta_n_STD = nan(medSize,medSize);
for i = 2:medSize-1
    for j = 2:medSize-1
        thetaCheck = theta_n(j-1:j+1,i-1:i+1);
        theta_n_STD(j,i) = std(thetaCheck(isnan(thetaCheck)==0));
    end
end
theta_n(theta_n_STD>stdThreshold) = NaN;


% Buoyant feeder
data_fd = importdata('..\Exp_Prev_C1\Data_27-11400-11500\B00001.dat');
data_fd = data_fd.data;
xf = (data_fd(:,1) - xCorrect)/1000;
yf = (data_fd(:,2) - yCorrect)/1000;
uf = data_fd(:,3);
vf = data_fd(:,4);
noInd = find(uf==0&vf==0);
xf(noInd) = [];
yf(noInd) = [];
uf(noInd) = [];
vf(noInd) = [];

xlin = linspace(min(xf),max(xf),medSize);
ylin = linspace(min(yf),max(yf),medSize);
xfM = nan(medSize,medSize);
yfM = nan(medSize,medSize);
ufM = nan(medSize,medSize);
vfM = nan(medSize,medSize);
for i = 2:medSize-1
    for j = 2:medSize-1
        indCheck = xf>xlin(i-1) & xf<xlin(i+1) & yf>ylin(j-1) & yf<ylin(j+1);
        xCheck = xf(indCheck);
        yCheck = yf(indCheck);
        vxCheck = uf(indCheck);
        vyCheck = vf(indCheck);
        xfM(j,i) = median(xCheck(isnan(xCheck)==0));
        yfM(j,i) = median(yCheck(isnan(xCheck)==0));
        ufM(j,i) = median(vxCheck(isnan(xCheck)==0));
        vfM(j,i) = median(vyCheck(isnan(xCheck)==0));
    end
end
xf = xfM;
yf = yfM;
uf = ufM;
vf = vfM;
clear *M *Check;

vector_f = sqrt(uf.^2+vf.^2);
theta_f = acos(uf./vector_f);

theta_f_STD = nan(medSize,medSize);
for i = 2:medSize-1
    for j = 2:medSize-1
        thetaCheck = theta_f(j-1:j+1,i-1:i+1);
        theta_f_STD(j,i) = std(thetaCheck(isnan(thetaCheck)==0));
    end
end
theta_f(theta_f_STD>stdThreshold) = NaN;


theta_f(yf>-0.020) = NaN;
theta_f(yf<-0.330) = NaN;
theta_n(theta_n>1.66&yn<-0.2) = NaN;
theta_f(theta_f>1.66&yf<-0.2) = NaN;

theta_n(theta_n<pi/2) = pi - theta_n(theta_n<pi/2);
theta_n(xn>0) = NaN;
theta_f(theta_f<pi/2) = pi - theta_f(theta_f<pi/2);
theta_f(xf>0) = NaN;
xn(xn>0) = NaN;
xf(xf>0) = NaN;

dy = [yn(2:end,:)-yn(1:end-1,:);yf(2:end,:)-yf(1:end-1,:)];
dy = mean(dy(isnan(dy)==0));


%% Simulate profiles construct

excludeBounds = 5;
skipInd1 = [find(isnan(xn(88,:))==0,1,'first'),...
    find(isnan(xf(88,:))==0,1,'first')];
skipInd2 = [find(isnan(xn(88,:))==0,1,'last'),...
    find(isnan(xf(88,:))==0,1,'last')];
skipInd1 = max(skipInd1);
skipInd2 = min(skipInd2);



Lp = round(logspace(log10(10),log10(90),9),0);
Lpstar = Lp*dy/Lb;
expB = [1,2,3,4];
thetaMin = linspace(pi/2,pi,19);
dtheta = linspace(0,pi/2,19);

resid_n = nan(size(theta_n,1),size(theta_n,2),length(Lp),length(dtheta),length(expB));
resid_f = resid_n;
std_n = resid_n;
std_f = resid_n;
h = waitbar(0,'Processing...');
for m1 = 1:length(Lp)
for m3 = 1:length(dtheta)
for m4 = 1:length(expB)
for d1 = 1:size(theta_n,1)-Lp(m1)
for d2 = skipInd1+excludeBounds:skipInd2-excludeBounds
    
    yp = (0:Lp(m1)-1)/(Lp(m1)-1);
    theta_extract_n = theta_n(d1:d1+Lp(m1)-1,d2);
    checky_n = yp(isnan(theta_extract_n)==0);
    checkt_n = theta_extract_n(isnan(theta_extract_n)==0);
    theta_extract_f = theta_f(d1:d1+Lp(m1)-1,d2);
    checky_f = yp(isnan(theta_extract_f)==0);
    checkt_f = theta_extract_f(isnan(theta_extract_f)==0);
    
    
    resid0_n = 10000000;
    std0_n = NaN;
    resid0_f = resid0_n;
    std0_f = std0_n;
    for m2 = 1:length(thetaMin)
        theta_sim_n = thetaMin(m2) + dtheta(m3)*checky_n.^exp(m4);
        checkresid_n = abs(tan(theta_sim_n'-checkt_n));
        theta_sim_f = thetaMin(m2) + dtheta(m3)*checky_f.^exp(m4);
        checkresid_f = abs(tan(theta_sim_f'-checkt_f));
        
        if mean(checkresid_n(isnan(checkresid_n)==0)) < resid0_n
            resid0_n = mean(checkresid_n(isnan(checkresid_n)==0));
            std0_n = std(checkresid_n(isnan(checkresid_n)==0));
        end
        if mean(checkresid_f(isnan(checkresid_f)==0)) < resid0_f
            resid0_f = mean(checkresid_f(isnan(checkresid_f)==0));
            std0_f = std(checkresid_f(isnan(checkresid_f)==0));
        end
    end
    
    if resid0_n == 10000000
        resid0_n = NaN;
    end
    if resid0_f == 10000000
        resid0_f = NaN;
    end
    
    resid_n(d1,d2,m1,m3,m4) = resid0_n;
    std_n(d1,d2,m1,m3,m4) = std0_n;
    resid_f(d1,d2,m1,m3,m4) = resid0_f;
    std_f(d1,d2,m1,m3,m4) = std0_f;
end
end
end
waitbar((m1-1)/length(Lp) + m3/length(Lp)/length(dtheta),h);
end
end
close(h);


%% Best params

best_dtheta_n = nan(size(xn,1),size(xn,2),length(Lp));
best_expB_n = best_dtheta_n;
best_dtheta_f = best_dtheta_n;
best_expB_f = best_dtheta_n;
for d1 = 1:size(theta_n,1)
for d2 = skipInd1+excludeBounds:skipInd2-excludeBounds
for m1 = 1:length(Lp)
    checkResid = resid_n(d1,d2,m1,:,:);
    minInd = find(checkResid==min(checkResid,[],'all'));
    [ind1,ind2,indL,indd,indB] = ind2sub(size(checkResid),minInd);
    if length(unique(indd)) == 1
        best_dtheta_n(d1,d2,m1) = dtheta(indd(1));
    else
%         best_dtheta_n(d1,d2,m1) = 2;
    end
    if length(unique(indB)) == 1
        best_expB_n(d1,d2,m1) = expB(indB(1));
    else
%         best_expB_n(d1,d2,m1) = 2;
    end

    
    checkResid = resid_f(d1,d2,m1,:,:);
    minInd = find(checkResid==min(checkResid,[],'all'));
    [ind1,ind2,indL,indd,indB] = ind2sub(size(checkResid),minInd);
    if length(unique(indd)) == 1
        best_dtheta_f(d1,d2,m1) = dtheta(indd(1));
    else
%         best_dtheta_f(d1,d2,m1) = 2;
    end
    if length(unique(indB)) == 1
        best_expB_f(d1,d2,m1) = expB(indB(1));
    else
%         best_expB_f(d1,d2,m1) = 2;
    end
end
end
end



binMat_n = nan(length(dtheta),length(expB),length(Lp));
binMat_f = binMat_n;
for m1 = 1:length(Lp)
for m2 = 1:length(dtheta)
for m3 = 1:length(expB)
    binInd = best_dtheta_n(:,:,m1)==dtheta(m2) & best_expB_n(:,:,m1)==expB(m3);
    binMat_n(m2,m3,m1) = sum(sum(binInd));
    
    binInd = best_dtheta_f(:,:,m1)==dtheta(m2) & best_expB_f(:,:,m1)==expB(m3);
    binMat_f(m2,m3,m1) = sum(sum(binInd));
end
end
    binMat_n(:,:,m1) = binMat_n(:,:,m1)/sum(sum(binMat_n(:,:,m1)));
    binMat_f(:,:,m1) = binMat_f(:,:,m1)/sum(sum(binMat_f(:,:,m1)));
end

transp_n = binMat_n~=0;
transp_f = binMat_f~=0;
transp_class = nan(size(transp_n));
for i = 1:size(transp_class,3)
    transp_slot = binMat_n(:,:,i)+binMat_f(:,:,i);
    transp_slot = transp_slot/sum(sum(transp_slot));
    transp_slot(transp_slot==0) = NaN;
    transp_slot = log10(transp_slot);
    transp_slot = transp_slot - min(min(transp_slot));
    transp_class(:,:,i) = transp_slot/max(max(transp_slot));
    
end
binMat_class = 2*binMat_n./(binMat_n+binMat_f) - 1;


%% Plot 1 Theta

xlimits = [-0.100,0];
ylimits = [-0.370,0];
quivSkip = 6;
quivSize = 1;

figure(1); clf;
set(gcf,'units','normalized','outerposition',[0 0.2 0.7 0.7]);
subplot(1,2,1);
    scatter(reshape(xn,numel(xn),1),...
        reshape(yn,numel(yn),1),30,...
        reshape(theta_n,numel(theta_n),1),'filled'); hold on;
    quiver(xn(1:quivSkip:end,1:quivSkip:end),yn(1:quivSkip:end,1:quivSkip:end),...
        un(1:quivSkip:end,1:quivSkip:end)./vector_n(1:quivSkip:end,1:quivSkip:end),...
        vn(1:quivSkip:end,1:quivSkip:end)./vector_n(1:quivSkip:end,1:quivSkip:end),...
        quivSize,'w');
    
    ylabel('y (mm)');
    title('Non-feeder','FontWeight','normal');
subplot(1,2,2);
    scatter(reshape(xf,numel(xf),1),...
        reshape(yf,numel(yf),1),30,...
        reshape(theta_f,numel(theta_f),1),'filled'); hold on;
    quiver(xf(1:quivSkip:end,1:quivSkip:end),yf(1:quivSkip:end,1:quivSkip:end),...
        uf(1:quivSkip:end,1:quivSkip:end)./vector_f(1:quivSkip:end,1:quivSkip:end),...
        vf(1:quivSkip:end,1:quivSkip:end)./vector_f(1:quivSkip:end,1:quivSkip:end),...
        quivSize,'w');
    title('Feeder','FontWeight','normal');
    
for i = 1:2
    subplot(1,2,i);
    caxis([pi/2,pi]);
    xlim(xlimits); ylim(ylimits);
    xlabel('x (mm)');
    set(gca,'Fontsize',labelSzAx);
    c=colorbar;
end
set(c,'Ticks',[pi/2,pi],'TickLabels',[90,180]);
ylabel(c,'\theta (deg)');


%% Plot 2 Classify Lp* plots

spot = [1,5,9];

xloc = repmat([0.09 0.38 0.67],1,3);
yloc = [0.69*ones(1,3),0.40*ones(1,3),0.11*ones(1,3)];
wd = 0.25; ht = 0.24;

figure(2); clf;
set(gcf,'Units','normalized','OuterPosition',[0 0.05 1 0.95]);
for i = 1:3
    subplot(3,3,i);
    showBin = log10(binMat_n(:,:,spot(i)));
    showTransp = transp_n(:,:,spot(i));
    imagesc(showBin,'AlphaData',showTransp);
    title(sprintf('L_p* = %0.2f',round(Lpstar(spot(i)),2)),'FontWeight','normal');
    
    subplot(3,3,i+3);
    showBin = log10(binMat_f(:,:,spot(i)));
    showTransp = transp_f(:,:,spot(i));
    imagesc(showBin,'AlphaData',showTransp);
    
    subplot(3,3,i+6);
    showBin = binMat_class(:,:,spot(i));
    showTransp = transp_class(:,:,spot(i));
    imagesc(showBin,'AlphaData',showTransp);
end
    
%
for i = 1:9
    subplot(3,3,i);
    shading flat;
%     c=colorbar;
    set(gca,'Fontsize',14);
    
    set(gca,'Xtick',expB);
    set(gca,'YTick',1:6:19)
    ytickies = get(gca,'Ytick');
    set(gca,'YTickLabel',dtheta(ytickies)*180/pi);
    if i > 6
        xlabel('b'); caxis([-1,1.1]);
    else
        set(gca,'XTickLabel',[]);
        caxis([-3,0]);
    end
    if i == 1
        ylabel({'Non-feeder';'\Delta\theta'});
    elseif i == 4
        ylabel({'Feeder';'\Delta\theta'});
    elseif i == 7
        ylabel({'Non-fdr./Feeder';'\Delta\theta'});
    else
        set(gca,'YTickLabel',[]);
    end
    if i == 3
        c=colorbar;
        ylabel(c,'Abundance (log %)');
    elseif i == 6
        c=colorbar;
        ylabel(c,'Abundance (log %)')
    elseif i == 9
        c=colorbar;
        ylabel(c,'Classification')
    end
    
    xp = 4; yp = 16;
    if i == 1
        tx=text(xp,yp,'i'); set(tx,'FontSize',20);
    elseif i == 2
        tx=text(xp,yp,'ii'); set(tx,'FontSize',20);
    elseif i == 3
        tx=text(xp,yp,'iii'); set(tx,'FontSize',20);
    elseif i == 4
        tx=text(xp,yp,'iv'); set(tx,'FontSize',20);
    elseif i == 5
        tx=text(xp,yp,'v'); set(tx,'FontSize',20);
    elseif i == 6
        tx=text(xp,yp,'vi'); set(tx,'FontSize',20);
    elseif i == 7
        tx=text(xp,yp,'vii'); set(tx,'FontSize',20);
    elseif i == 8
        tx=text(xp,yp,'viii'); set(tx,'FontSize',20);
    elseif i == 9
        tx=text(xp,yp,'ix'); set(tx,'FontSize',20);
    end
    
    pause(0.001);
    set(gca,'Position',[xloc(i),yloc(i),wd,ht]);
end


%% Plot 3 Classify b plots

xloc = repmat([0.10 0.30 0.50 0.70],1,3);
yloc = [0.67*ones(1,4),0.39*ones(1,4),0.11*ones(1,4)];
wd = 0.17; ht = 0.24;

figure(3); clf;
set(gcf,'Units','normalized','OuterPosition',[0 0.05 1 0.95]);
for i = 1:size(binMat_n,2)

    subplot(3,size(binMat_n,2),i);
    showBin = reshape(binMat_n(:,i,:),size(binMat_n,1),size(binMat_n,3));
    showTransp = reshape(transp_n(:,i,:),size(binMat_n,1),size(binMat_n,3));
    imagesc(log10(showBin),'AlphaData',showTransp);
    title(sprintf('b = %0d',expB(i)),'FontWeight','normal');
    
    subplot(3,size(binMat_n,2),i+size(binMat_n,2));
    showBin = reshape(binMat_f(:,i,:),size(binMat_n,1),size(binMat_n,3));
    showTransp = reshape(transp_f(:,i,:),size(binMat_n,1),size(binMat_n,3));
    imagesc(log10(showBin),'AlphaData',showTransp);
    
    subplot(3,size(binMat_n,2),i+size(binMat_n,2)*2);
    showBin = reshape(binMat_class(:,i,:),size(binMat_n,1),size(binMat_n,3));
    showTransp = reshape(transp_class(:,i,:),size(binMat_n,1),size(binMat_n,3));
    imagesc(showBin,'AlphaData',showTransp);
end

%
for i = 1:size(binMat_n,2)*3
    subplot(3,size(binMat_n,2),i);
    shading flat;
%     c=colorbar;
    set(gca,'Fontsize',14);
    
    set(gca,'Xtick',1:2:length(Lp));
    set(gca,'YTick',1:6:19);
    xtickies = get(gca,'XTick');
    ytickies = get(gca,'YTick');
    set(gca,'XTickLabel',round(Lpstar(xtickies),2));
    set(gca,'YTickLabel',dtheta(ytickies)*180/pi);
    if i > size(binMat_n,2)*2
        xlabel('L_p*'); caxis([-1,1.1]);
    else
        set(gca,'XTickLabel',[]);
        caxis([-3,0]);
    end
    if i == 1
        ylabel({'Non-feeder';'\Delta\theta'});
    elseif i == 5
        ylabel({'Feeder';'\Delta\theta'});
    elseif i == 9
        ylabel({'Non-fdr./Feeder';'\Delta\theta'});
    else
        set(gca,'YTickLabel',[]);
    end
    if i == 4
        c=colorbar;
        ylabel(c,'Abundance (log %)');
    elseif i == 8
        c=colorbar;
        ylabel(c,'Abundance (log %)')
    elseif i == 12
        c=colorbar;
        ylabel(c,'Classification')
    end
    
    xp = 8.2; yp = -1.2;
    if i == 1
        tx=text(xp,yp,'i'); set(tx,'FontSize',20);
    elseif i == 2
        tx=text(xp,yp,'ii'); set(tx,'FontSize',20);
    elseif i == 3
        tx=text(xp,yp,'iii'); set(tx,'FontSize',20);
    elseif i == 4
        tx=text(xp,yp,'iv'); set(tx,'FontSize',20);
    elseif i == 5
        tx=text(xp,yp,'v'); set(tx,'FontSize',20);
    elseif i == 6
        tx=text(xp,yp,'vi'); set(tx,'FontSize',20);
    elseif i == 7
        tx=text(xp,yp,'vii'); set(tx,'FontSize',20);
    elseif i == 8
        tx=text(xp,yp,'viii'); set(tx,'FontSize',20);
    elseif i == 9
        tx=text(xp,yp,'ix'); set(tx,'FontSize',20);
    elseif i == 10
        tx=text(xp,yp,'x'); set(tx,'FontSize',20);
    elseif i == 11
        tx=text(xp,yp,'xi'); set(tx,'FontSize',20);
    elseif i == 12
        tx=text(xp,yp,'xii'); set(tx,'FontSize',20);
    end
    
    pause(0.001);
    set(gca,'Position',[xloc(i),yloc(i),wd,ht]);
end


%% Saving

if saveArg == 1
    save('results.mat');
    
    figure(1);
    outFile = 'theta';
    print(gcf,outFile,'-djpeg','-r300');
    saveas(gcf,outFile,'epsc');
    saveas(gcf,outFile,'fig');
    
    figure(2);
    outFile = 'classify dth v expB';
    print(gcf,outFile,'-djpeg','-r300');
    saveas(gcf,outFile,'epsc');
    saveas(gcf,outFile,'fig');
    
    figure(3);
    outFile = 'classify dth v Lp';
    print(gcf,outFile,'-djpeg','-r300');
    saveas(gcf,outFile,'epsc');
    saveas(gcf,outFile,'fig');
end



